// <copyright file="PathFinderFast.cs" company="Gustavo Franco">
// Copyright (c) 2006 All Right Reserved
// </copyright>
// <author>Gustavo Franco</author>
// <email>gustavo_franco@hotmail.com</email>
// <date>2006</date>
// <summary>Path finder algorithm which uses the fast method for pathfinding i.e.: Faster but less accurate.</summary>

// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
// KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR
// PURPOSE. IT CAN BE DISTRIBUTED FREE OF CHARGE AS LONG AS THIS HEADER
// REMAINS UNCHANGED.

// Email:  gustavo_franco@hotmail.com

// Copyright (C) 2006 Franco, Gustavo

// Work done by Robert Tenney on this library: PathFinder, is based off of the previous work
// done by: Franco, Gustavo.

using System;
using System.Collections.Generic;
using System.Drawing;
using System.Runtime.InteropServices;

namespace TenneySoftware.Engine.Algorithms.Pathfinder {
    /// <summary>
    /// Path finder algorithm which uses the fast method for pathfinding i.e.: Faster but less accurate.
    /// </summary>
    [Author("Franco, Gustavo")]
    public class PathfinderFast : IPathfinder {
        #region Variables Declaration
        /// <summary>
        /// Grid for pathfinding in.
        /// </summary>
        [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Performance", "CA1814:PreferJaggedArraysOverMultidimensional", MessageId = "Member", Justification = "Multidimensional preferred.")]
        private byte[,] grid = null;

        /// <summary>
        /// Queue of open location.
        /// </summary>
        private PriorityQueueB<int> open;

        /// <summary>
        /// Collection of closed nodes.
        /// </summary>
        private PathfinderNodeCollection close = new PathfinderNodeCollection();

        /// <summary>
        /// Was stop called.
        /// </summary>
        private bool stop;

        /// <summary>
        /// Are we stopped.
        /// </summary>
        private bool stopped = true;

        /// <summary>
        /// Horizontal line being scanned.
        /// </summary>
        private int horizontal;

        /// <summary>
        /// Formula to use for pathfinding.
        /// </summary>
        private HeuristicFormula formula = HeuristicFormula.Manhattan;

        /// <summary>
        /// Are we allowing diagonals.
        /// </summary>
        private bool diagonals = true;

        /// <summary>
        /// Heuristic estimate value.
        /// </summary>
        private int heuristicEstimate = 2;

        /// <summary>
        /// Should we make directional changes heavier.
        /// </summary>
        private bool punishChangeDirection;

        /// <summary>
        /// Should we use a tibreaker in the event of a tie.
        /// </summary>
        private bool tieBreaker;

        /// <summary>
        /// Should we make the diagonals heavier.
        /// </summary>
        private bool heavyDiagonals;

        /// <summary>
        /// The farthest distance away to search.
        /// </summary>
        private int searchLimit = 2000;

        /// <summary>
        /// Time taken to complete pathfinding.
        /// </summary>
        private double completedTime;

        /// <summary>
        /// Calculation grid.
        /// </summary>
        private PathFinderNodeFast[] calcGrid;

        /// <summary>
        /// Default value for open nodes.
        /// </summary>
        private byte openNodeValue = 1;

        /// <summary>
        /// Default value for close nodes.
        /// </summary>
        private byte closeNodeValue = 2;

        // Promoted local variables to member variables to avoid recreation between calls

        /// <summary>
        /// H value for calculation.
        /// </summary>
        private int valueH;

        /// <summary>
        /// The current location.
        /// </summary>
        private int location;

        /// <summary>
        /// The new location.
        /// </summary>
        private int newLocation;

        /// <summary>
        /// Location's X position.
        /// </summary>
        private ushort locationX;

        /// <summary>
        /// Location's Y position.
        /// </summary>
        private ushort locationY;

        /// <summary>
        /// New location's X position.
        /// </summary>
        private ushort newLocationX;

        /// <summary>
        /// New location's Y position.
        /// </summary>
        private ushort newLocationY;

        /// <summary>
        /// Counter for closed nodes.
        /// </summary>
        private int closeNodeCounter;

        /// <summary>
        /// Grid's X location.
        /// </summary>
        private ushort gridX;

        /// <summary>
        /// Grid's Y location.
        /// </summary>
        private ushort gridY;

        /// <summary>
        /// Grid's X formulation.
        /// </summary>
        private ushort gridXMinus1;

        /// <summary>
        /// Grid's Y formulation.
        /// </summary>
        private ushort gridYLog2;

        /// <summary>
        /// Have we found a solution.
        /// </summary>
        private bool found;

        /// <summary>
        /// Directional matrix.
        /// </summary>
        [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Performance", "CA1814:PreferJaggedArraysOverMultidimensional", MessageId = "Member", Justification = "Multidimensional preferred.")]
        private sbyte[,] direction = new sbyte[8, 2] { { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }, { 1, -1 }, { 1, 1 }, { -1, 1 }, { -1, -1 } };

        /// <summary>
        /// End location.
        /// </summary>
        private int endLocation;

        /// <summary>
        /// New G value.
        /// </summary>
        private int newG;
        #endregion

        #region Constructors
        /// <summary>
        /// Initializes a new instance of the PathfinderFast class.
        /// </summary>
        /// <param name="grid">Pathfinding field.</param>
        public PathfinderFast(byte[,] grid) {
            if (grid == null) {
                throw new ArgumentNullException("grid", "Grid cannot be null");
            }
            
            this.grid = grid;
            this.gridX = (ushort)(grid.GetUpperBound(0) + 1);
            this.gridY = (ushort)(grid.GetUpperBound(1) + 1);
            this.gridXMinus1 = (ushort)(this.gridX - 1);
            this.gridYLog2 = (ushort)Math.Log(this.gridY, 2);

            // This should be done at the constructor, for now we leave it here.
            if (Math.Log(this.gridX, 2) != (int)Math.Log(this.gridX, 2) ||
                Math.Log(this.gridY, 2) != (int)Math.Log(this.gridY, 2)) {
                throw new ArgumentException("Invalid Grid, size in X and Y must be power of 2");
            }
            
            if (this.calcGrid == null || this.calcGrid.Length != (this.gridX * this.gridY)) {
                this.calcGrid = new PathFinderNodeFast[this.gridX * this.gridY];
            }
            
            this.open = new PriorityQueueB<int>(new ComparePFNodeMatrix(this.calcGrid));
        }
        #endregion

        #region Properties
        /// <summary>
        /// Gets a value indicating whether the algorithm was stopped or not.
        /// </summary>
        /// <value>
        /// A value indicating whether the algorithm was stopped or not.
        /// </value>
        public bool Stopped {
            get {
                return this.stopped;
            }
        }

        /// <summary>
        /// Gets or sets the formula used for heuristics.
        /// </summary>
        /// <value>
        /// The formula used for heuristics.
        /// </value>
        public HeuristicFormula Formula {
            get {
                return this.formula;
            }
            
            set {
                this.formula = value;
            }
        }

        /// <summary>
        /// Gets or sets a value indicating whether or not to allow diagonals.
        /// </summary>
        /// <value>
        /// A value indicating whether or not to allow diagonals.
        /// </value>
        [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Performance", "CA1814:PreferJaggedArraysOverMultidimensional", MessageId = "Body", Justification = "Multidimensional preferred.")]
        public bool Diagonals {
            get {
                return this.diagonals;
            }
            
            set {
                this.diagonals = value;
                if (this.diagonals) {
                    this.direction = new sbyte[8, 2] { { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }, { 1, -1 }, { 1, 1 }, { -1, 1 }, { -1, -1 } };
                } else {
                    this.direction = new sbyte[4, 2] { { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 } };
                }
            }
        }

        /// <summary>
        /// Gets or sets a value indicating whether or not to score diagonals heavily.
        /// </summary>
        /// <value>
        /// A value indicating whether or not to score diagonals heavily.
        /// </value>
        public bool HeavyDiagonals {
            get {
                return this.heavyDiagonals;
            }
            
            set {
                this.heavyDiagonals = value;
            }
        }

        /// <summary>
        /// Gets or sets a value for use by the heuristic formula for it's estimation.
        /// </summary>
        /// <value>
        /// A value for use by the heuristic formula for it's estimation.
        /// </value>
        public int HeuristicEstimate {
            get {
                return this.heuristicEstimate;
            }
            
            set {
                this.heuristicEstimate = value;
            }
        }

        /// <summary>
        /// Gets or sets a value indicating whether or not to score directional changes more heavily.
        /// </summary>
        /// <value>
        /// A value indicating whether or not to score directional changes more heavily.
        /// </value>
        public bool PunishChangeDirection {
            get {
                return this.punishChangeDirection;
            }
            
            set {
                this.punishChangeDirection = value;
            }
        }

        /// <summary>
        /// Gets or sets a value indicating whether or not to perform additional math to break the tie.
        /// </summary>
        /// <value>
        /// A value indicating whether or not to perform additional math to break the tie.
        /// </value>
        public bool Tiebreaker {
            get {
                return this.tieBreaker;
            }
            
            set {
                this.tieBreaker = value;
            }
        }

        /// <summary>
        /// Gets or sets a value to set the search limit to.
        /// </summary>
        /// <value>
        /// A value to set the search limit to.
        /// </value>
        public int SearchLimit {
            get {
                return this.searchLimit;
            }
            
            set {
                this.searchLimit = value;
            }
        }

        /// <summary>
        /// Gets or sets the time taken to complete the algorithm.
        /// </summary>
        /// <value>
        /// The time taken to complete the algorithm.
        /// </value>
        public double CompletedTime {
            get {
                return this.completedTime;
            }
            
            set {
                this.completedTime = value;
            }
        }
        #endregion
        
        #region Methods
        /// <summary>
        /// Causes the path finder algorithm to stop if it is currently running.
        /// </summary>
        public void FindPathStop() {
            this.stop = true;
        }

        /// <summary>
        /// Starts the path finding algorithm.
        /// </summary>
        /// <param name="start">Start of the path.</param>
        /// <param name="destination">End of the path.</param>
        /// <returns>A list of nodes with the path finder's solution.</returns>
        public PathfinderNodeCollection FindPath(Point start, Point destination) {
            lock (this) {
                HighResolutionTimer.Start();

                // Is faster if we don't clear the matrix, just assign different values for open and close and ignore the rest
                // I could have user Array.Clear() but using unsafe code is faster, no much but it is.
                this.found = false;
                this.stop = false;
                this.stopped = false;
                this.closeNodeCounter = 0;
                this.openNodeValue += 2;
                this.closeNodeValue += 2;
                this.open.Clear();
                this.close.Clear();

                this.location = (start.Y << this.gridYLog2) + start.X;
                this.endLocation = (destination.Y << this.gridYLog2) + destination.X;
                this.calcGrid[this.location].G = 0;
                this.calcGrid[this.location].F = this.heuristicEstimate;
                this.calcGrid[this.location].PX = (ushort)start.X;
                this.calcGrid[this.location].PY = (ushort)start.Y;
                this.calcGrid[this.location].Status = this.openNodeValue;

                this.open.Push(this.location);
                while (this.open.Count > 0 && !this.stop) {
                    this.location = this.open.Pop();

                    // Is it in closed list? means this node was already processed
                    if (this.calcGrid[location].Status == this.closeNodeValue) {
                        continue;
                    }
                    
                    this.locationX   = (ushort)(this.location & this.gridXMinus1);
                    this.locationY   = (ushort)(this.location >> this.gridYLog2);

                    if (this.location == this.endLocation) {
                        this.calcGrid[location].Status = this.closeNodeValue;
                        this.found = true;
                        break;
                    }

                    if (this.closeNodeCounter > this.searchLimit) {
                        this.stopped = true;
                        this.completedTime = HighResolutionTimer.GetTime();
                        return null;
                    }

                    if (this.punishChangeDirection) {
                        this.horizontal = (this.locationX - this.calcGrid[location].PX);
                    }
                    
                    // Lets calculate each successors
                    for (int i = 0; i < (this.diagonals ? 8 : 4); i++) {
                        this.newLocationX = (ushort)(this.locationX + this.direction[i, 0]);
                        this.newLocationY = (ushort)(this.locationY + this.direction[i, 1]);
                        this.newLocation = (this.newLocationY << this.gridYLog2) + this.newLocationX;

                        if (this.newLocationX >= this.gridX || this.newLocationY >= this.gridY) {
                            continue;
                        }
                        
                        // Unbreakeable?
                        if (this.grid[this.newLocationX, this.newLocationY] == 0) {
                            continue;
                        }
                        
                        if (this.heavyDiagonals && i > 3) {
                            this.newG = this.calcGrid[location].G + (int)(this.grid[this.newLocationX, this.newLocationY] * 2.41);
                        } else {
                            this.newG = this.calcGrid[location].G + this.grid[this.newLocationX, this.newLocationY];
                        }
                        
                        if (this.punishChangeDirection) {
                            if ((this.newLocationX - this.locationX) != 0) {
                                if (this.horizontal == 0) {
                                    this.newG += Math.Abs(this.newLocationX - destination.X) + Math.Abs(this.newLocationY - destination.Y);
                                }
                            }
                            
                            if ((this.newLocationY - this.locationY) != 0) {
                                if (this.horizontal != 0) {
                                    this.newG += Math.Abs(this.newLocationX - destination.X) + Math.Abs(this.newLocationY - destination.Y);
                                }
                            }
                        }

                        // Is it open or closed?
                        if (this.calcGrid[newLocation].Status == this.openNodeValue || this.calcGrid[newLocation].Status == this.closeNodeValue) {
                            // The current node has less code than the previous? then skip this node
                            if (this.calcGrid[newLocation].G <= this.newG) {
                                continue;
                            }
                        }

                        this.calcGrid[newLocation].PX = this.locationX;
                        this.calcGrid[newLocation].PY = this.locationY;
                        this.calcGrid[newLocation].G = this.newG;

                        switch (this.formula) {
                            default:
                            case HeuristicFormula.Manhattan:
                                this.valueH = this.heuristicEstimate * (Math.Abs(this.newLocationX - destination.X) + Math.Abs(this.newLocationY - destination.Y));
                                break;
                            case HeuristicFormula.MaxDxDy:
                                this.valueH = this.heuristicEstimate * Math.Max(Math.Abs(this.newLocationX - destination.X), Math.Abs(this.newLocationY - destination.Y));
                                break;
                            case HeuristicFormula.DiagonalShortcut:
                                int horizontalDiagonal = Math.Min(Math.Abs(this.newLocationX - destination.X), Math.Abs(this.newLocationY - destination.Y));
                                int horizontalStraight = (Math.Abs(this.newLocationX - destination.X) + Math.Abs(this.newLocationY - destination.Y));
                                this.valueH = ((this.heuristicEstimate * 2) * horizontalDiagonal) + (this.heuristicEstimate * (horizontalStraight - (2 * horizontalDiagonal)));
                                break;
                            case HeuristicFormula.Euclidean:
                                this.valueH = (int)(this.heuristicEstimate * Math.Sqrt(Math.Pow((this.newLocationY - destination.X), 2) + Math.Pow((this.newLocationY - destination.Y), 2)));
                                break;
                            case HeuristicFormula.EuclideanNoSqr:
                                this.valueH = (int)(this.heuristicEstimate * (Math.Pow((this.newLocationX - destination.X), 2) + Math.Pow((this.newLocationY - destination.Y), 2)));
                                break;
                            case HeuristicFormula.Custom1:
                                Point dxy = new Point(Math.Abs(destination.X - this.newLocationX), Math.Abs(destination.Y - this.newLocationY));
                                int orthogonal = Math.Abs(dxy.X - dxy.Y);
                                int diagonal = Math.Abs(((dxy.X + dxy.Y) - orthogonal) / 2);
                                this.valueH = this.heuristicEstimate * (diagonal + orthogonal + dxy.X + dxy.Y);
                                break;
                        }
                        
                        if (this.tieBreaker) {
                            int dx1 = this.locationX - destination.X;
                            int dy1 = this.locationY - destination.Y;
                            int dx2 = start.X - destination.X;
                            int dy2 = start.Y - destination.Y;
                            int cross = Math.Abs((dx1 * dy2) - (dx2 * dy1));
                            this.valueH = (int)(this.valueH + (cross * 0.001));
                        }
                        
                        this.calcGrid[this.newLocation].F = this.newG + this.valueH;
                        this.open.Push(this.newLocation);
                        this.calcGrid[this.newLocation].Status = this.openNodeValue;
                    }

                    this.closeNodeCounter++;
                    this.calcGrid[this.location].Status = this.closeNodeValue;
                }

                this.completedTime = HighResolutionTimer.GetTime();
                if (this.found) {
                    this.close.Clear();
                    int posX = destination.X;
                    int posY = destination.Y;

                    PathFinderNodeFast fastNodeTmp = this.calcGrid[(destination.Y << this.gridYLog2) + destination.X];
                    PathfinderNode fastNode = new PathfinderNode();
                    fastNode.F  = fastNodeTmp.F;
                    fastNode.G  = fastNodeTmp.G;
                    fastNode.H  = 0;
                    fastNode.PX = fastNodeTmp.PX;
                    fastNode.PY = fastNodeTmp.PY;
                    fastNode.X  = destination.X;
                    fastNode.Y  = destination.Y;

                    while (fastNode.X != fastNode.PX || fastNode.Y != fastNode.PY) {
                        this.close.Add(fastNode);
                        posX = fastNode.PX;
                        posY = fastNode.PY;
                        fastNodeTmp = this.calcGrid[(posY << this.gridYLog2) + posX];
                        fastNode.F  = fastNodeTmp.F;
                        fastNode.G  = fastNodeTmp.G;
                        fastNode.H  = 0;
                        fastNode.PX = fastNodeTmp.PX;
                        fastNode.PY = fastNodeTmp.PY;
                        fastNode.X  = posX;
                        fastNode.Y  = posY;
                    }

                    this.close.Add(fastNode);
                    this.stopped = true;
                    return this.close;
                }
                
                this.stopped = true;
                return null;
            }
        }
        #endregion
        
        #region Structs
        /// <summary>
        /// Structure for holding the information related to each node.
        /// </summary>
        [Author("Franco, Gustavo")]
        [StructLayout(LayoutKind.Sequential, Pack = 1)]
        internal struct PathFinderNodeFast {
            #region Variables Declaration
            /// <summary>
            /// F = Gone + heuristic.
            /// </summary>
            public int F; // f = gone + heuristic

            /// <summary>
            /// G value for calculation.
            /// </summary>
            public int G;

            /// <summary>
            /// Parent X location.
            /// </summary>
            public ushort PX; // Parent

            /// <summary>
            /// Parent Y location.
            /// </summary>
            public ushort PY;

            /// <summary>
            /// Status of node.
            /// </summary>
            public byte Status;
            #endregion
        }
        #endregion

        #region Inner Classes
        /// <summary>
        /// PathfinderFast Node matrix comparer.
        /// </summary>
        [Author("Franco, Gustavo")]
        internal class ComparePFNodeMatrix : IComparer<int> {
            #region Variables Declaration
            /// <summary>
            /// Matrix of pathfinder nodes.
            /// </summary>
            private PathFinderNodeFast[] matrix;
            #endregion

            #region Constructors
            /// <summary>
            /// Initializes a new instance of the ComparePFNodeMatrix class.
            /// </summary>
            /// <param name="matrix">Matrix of nodes.</param>
            public ComparePFNodeMatrix(PathFinderNodeFast[] matrix) {
                this.matrix = matrix;
            }
            #endregion

            #region IComparer Members
            /// <summary>
            /// Compare two index with each other.
            /// </summary>
            /// <param name="a">First index.</param>
            /// <param name="b">Second index.</param>
            /// <returns>If a is less then b then a value less than zero will return. If equal, then zero will return.
            /// If greater then a value greater than zero will be returned.</returns>
            public int Compare(int a, int b) {
                if (this.matrix[a].F > this.matrix[b].F) {
                    return 1;
                } else if (this.matrix[a].F < this.matrix[b].F) {
                    return -1;
                }
                
                return 0;
            }
            #endregion
        }
        #endregion
    }
}
